#include "PID_inc.h"

PID_Inc::PID_Inc()
{
}

PID_Inc::PID_Inc(float p, float i, float d)
{
    SetPid(p, i, d);
}

PID_Inc::~PID_Inc()
{
}

float PID_Inc::Compute(float tar, float act)
{
#if false 
    float u_increment;

    target = tar;
    actual = act;
    e = target-actual;
    u_increment = A*e+B*e_pre_1+C*e_pre_2;
    e_pre_2 = e_pre_1;
    e_pre_1 = e;

    Limit(u_increment);

    return u_increment;
#else 
    float u_increment;

    target = tar;
    actual = act;

    float err = target - actual; 
    
    float P = kp * (err - e_pre_1);
    float I = ki * err;
    float D = kd * (err - 2 * e_pre_1 + e_pre_2);
    
    u_increment = (int)(P + I + D); //这里的加号就是增量式PID的体现
 
    e_pre_2 = e_pre_1;
    e_pre_1 = err;
    
    return u_increment;
#endif 
}

void PID_Inc::SetPid(float p, float i, float d)
{
    kp = p;
    ki = i;
    kd = d;

    A = kp+ki+kd;
    B = -2*kd-kp;
    C = kd;
    e = target-actual;
}

void PID_Inc::SetOutputLimitsEnabled(bool enabled)
{
    m_outLimitsEnabled = enabled;
}

void PID_Inc::SetOutputLimits(float min, float max)
{
    m_outMin = min; 
    m_outMax = max;
}

float PID_Inc::GetKp()
{
    return kp;
}

float PID_Inc::GetKi()
{
    return ki;
}

float PID_Inc::GetKd()
{
    return kd;
}

void PID_Inc::Limit(float &val)
{
    if(m_outLimitsEnabled)
        return;
    
    if(val > m_outMax)
    {
        val = m_outMax;
    }
    else if(val < m_outMin)
    {
        val = m_outMin;
    }
}
